using System;
using System.Collections.Generic;
using System.Text;
using System.Xml;
using System.Reflection;
using System.Threading;

namespace HubertTestProgram
{
    public class Body
    {
        #region Fields
        private WebCam webCam;
        private BasicStamp basicStamp;
        private ServoMotor baseRotationServo;
        private ServoMotor shoulderServo;
        private ServoMotor elbowServo;
        private ServoMotor handServo;
        private ServoMotor headPanServo;
        private ServoMotor headTiltServo;


        private Thread sensorPollingThread;
        private double sensorPollingThreadSleepInterval;
        private bool running;
        #endregion

        #region Constructors
        public Body()
        {
            webCam = new WebCam();
            basicStamp = new BasicStamp();
            baseRotationServo = new ServoMotor();
            shoulderServo = new ServoMotor();
            elbowServo = new ServoMotor();
            handServo = new ServoMotor();
            headPanServo = new ServoMotor();
            headTiltServo = new ServoMotor();
        }        
        #endregion

        #region Private methods
        private void SensorPollingLoop()
        {
            while (running)
            {
                Robot.Instance.Body.WebCam.UpdateReading();
                Thread.Sleep((int)(sensorPollingThreadSleepInterval*1000));
            }
        }

        #endregion
        
        #region Public methods
        public void Initialize()
        {
            webCam.Initialize();
            basicStamp.Initialize();
        }

        public void Start()
        {
            webCam.Start();
            running = true;
            sensorPollingThread = new Thread(SensorPollingLoop);
            sensorPollingThread.Start();
        }

        public void Stop()
        {
            running = false;
            sensorPollingThread.Join();
        }

        public void Disconnect()
        {
            webCam.Disconnect();
            basicStamp.Disconnect();
        }
        #endregion

        #region Properties
        public WebCam WebCam
        {
            get { return webCam; }
        }
        public ServoMotor BaseRotationServo
        {
            get { return baseRotationServo;}
        }
        public ServoMotor ShoulderServo
        {
            get { return shoulderServo; }
        }
        public ServoMotor ElbowServo
        {
            get { return elbowServo; }
        }
        public ServoMotor HandServo
        {
            get { return handServo; }
        }
        public ServoMotor HeadPanServo
        {
            get { return headPanServo; }
        }
        public ServoMotor HeadTiltServo
        {
            get { return headTiltServo; }
        }
        public BasicStamp BasicStamp
        {
            get { return basicStamp; }
        }
        public double SensorPollingThreadSleepInterval
        {
            get { return sensorPollingThreadSleepInterval; }
            set { sensorPollingThreadSleepInterval = value; }
        }
        #endregion
    }
}
